
import numpy as np

def normalize_pts_2D(pts):
    """
    对输入的2D数据点进行规范化，处理后的均值为0，方差为sqrt(2)
    输入的是二维数组 ptr为N行3列（或2列）。
    """
    pts=np.array([i[:2] for i in pts])    #只取x,y 齐次量不要
    m=np.mean(pts,0)  #0表示按一个维度取均值，得到2x1
    s=np.std(pts, 0,ddof=1)   #取标准差  2x1
    s=s+[int(x==0) for x in s]  #若方差有0的，改为1
    #进行z-score标准化
    pts_norm= [np.sqrt(2)*(i-m)/s for i in pts]
    C=[[np.sqrt(2)/s[0],0, -np.sqrt(2)*m[0]/s[0]],
       [0,np.sqrt(2)/s[1], -np.sqrt(2)*m[1]/s[1]],
       [0,0,1] ]
    return C,[np.concatenate((i,[1]),0) for i in pts_norm]    #添加上齐次坐标值‘1’
    
def compute_H_from_xs(xs1,xs2):
    """计算H矩阵 """
    #规范化
    C1,xs1=normalize_pts_2D(xs1)
    C2,xs2=normalize_pts_2D(xs2)
    n=np.shape(xs1)[0]  #获得行数，即数据个数
    D=np.zeros((2*n,9))
    #生成D矩阵
    for i in np.arange(n):
        p1=xs1[i];   p2=xs2[i]
        D[2*i]=[p1[0]*p2[2], p1[1]*p2[2], p1[2]*p2[2], 0,0,0, -p1[0]*p2[0], -p1[1]*p2[0], -p1[2]*p2[0] ]
        D[2*i+1] =[0,0,0,p1[0]*p2[2], p1[1]*p2[2], p1[2]*p2[2], -p1[0]*p2[1], -p1[1]*p2[1], -p1[2]*p2[1] ]
    U, s, V=np.linalg.svd(D,full_matrices=True)   #SVD分解
    nullspace_dimension=np.sum([int(i<eps*s[0]*1e3) for i in s])
    if nullspace_dimension>1:
        print('Nullspace is a bit roomy...')
    h=V[8]  #解为V的最后一行，即第9行
    H=np.reshape(h,(3,3))
    H=np.dot(np.linalg.inv(C2),np.dot(H,C1))    #使H还原到原始数据的变换
    H=H/H[2][2]
    return H

##test
eps=2.2204e-16
a=[[40.705,451.897,1],[230.051,433.944,1],[399.644,446.819,1],[337.836,320.302,1],[230.496,286.148,1],[315.717,274.844,1]]
b=[[18.8581,488.391,1],[235.197,468.358,1],[429.135,487.011,1],[351.913,331.322,1],[237.751,292.093,1],[326.798,280.326,1]]

H=compute_H_from_xs(a,b)



